/**  
 * Copyright (c) 2010 University of Pennsylvania.
 *     All rights reserved.
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing,
 *  software distributed under the License is distributed on an "AS
 *  IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
 *  express or implied.  See the License for the specific language
 *  governing permissions and limitations under the License.
 *
 */
 
void init_mmarshal()
{
  marshal_checksize = 0;
}

void marshal_byte(uint8_t **pos, uint8_t byte)
{
DO_STACKTEST

  if (marshal_checksize == 0)
    **pos = byte;
  (*pos)++;
}

uint8_t unmarshal_byte(uint8_t **pos)
{
DO_STACKTEST

  return *((*pos)++);
}

void marshal_buffer(uint8_t **pos, void *buf, uint8_t size)
{
DO_STACKTEST

  if (marshal_checksize == 0)
    memcpy(*pos, buf, size);
  *pos += size;
}

void unmarshal_buffer(uint8_t **pos, void *buf, uint8_t size)
{
DO_STACKTEST

  memcpy(buf, *pos, size);
  *pos += size;
}

void marshal_word(uint8_t **pos, uint16_t word)
{
  marshal_byte(pos, word & 0xff);
  marshal_byte(pos, word >> 8);
}

uint16_t unmarshal_word(uint8_t **pos)
{
  return unmarshal_byte(pos) | ((uint16_t) unmarshal_byte(pos) << 8);
}

void marshal_long(uint8_t **pos, uint32_t l)
{
  marshal_byte(pos, l & 0xff);
  marshal_byte(pos, l >> 8);
  marshal_byte(pos, l >> 16);
  marshal_byte(pos, l >> 24);  
}

uint32_t unmarshal_long(uint8_t **pos)
{
  return unmarshal_byte(pos) | ((uint32_t) unmarshal_byte(pos) << 8) | ((uint32_t) unmarshal_byte(pos) << 16) | ((uint32_t) unmarshal_byte(pos) << 24);
}

void marshal_wordvb(uint8_t **pos, uint32_t word)
{
  uint8_t t;
  
  do {
    t = word & 0x7f;
    word >>= 7;
    marshal_byte(pos, (word == 0) ? t : (t | 0x80));
  } while (word != 0);
}

uint32_t unmarshal_wordvb(uint8_t **pos)
{
  uint32_t t, word = 0;
  uint8_t n = 0;

  do {
    t = unmarshal_byte(pos);
    word |= ((t & 0x7f) << n);
    n += 7;
  } while (t & 0x80);

  return word;
}

void marshal_summary(uint8_t **pos, SUMMARY *s)               // marshal summary into a buffer
{
  if (s == NULL)
    return;

  marshal_byte(pos, s->type);
  marshal_byte(pos, s->options);
  marshal_wordvb(pos, s->count);

  if (s->count <= SUMMARY_LEN)
    marshal_buffer(pos, s->summary, sizeof(SUMMARY_VALUE)*s->count);
  else marshal_buffer(pos, s->summary, sizeof(s->summary));
}

void unmarshal_summary(uint8_t **pos, SUMMARY *s)    // unmarshal summary from a buffer
{
  if (s == NULL)
    return;

  memset(s->summary, 0, sizeof(s->summary));

  s->type = unmarshal_byte(pos);
  s->options = unmarshal_byte(pos);
  s->count = unmarshal_wordvb(pos);
  if (s->count <= SUMMARY_LEN)
    unmarshal_buffer(pos, s->summary, sizeof(SUMMARY_VALUE)*s->count);
  else unmarshal_buffer(pos, s->summary, sizeof(s->summary));
}

void marshal_addr(uint8_t **pos, ADDRESS addr)
{
#if ADDRDOMAIN_BITS == 8
  marshal_byte(pos, addr);
#else
  marshal_word(pos, addr);
#endif
}

ADDRESS unmarshal_addr(uint8_t **pos)
{
#if ADDRDOMAIN_BITS == 8
  return unmarshal_byte(pos);
#else
  return unmarshal_word(pos);
#endif
}

void marshal_addrs(uint8_t **pos, ADDRESS addr1, ADDRESS addr2)
{
#if ADDRDOMAIN_BITS == 8
  marshal_byte(pos, addr1); marshal_byte(pos, addr2);
#elif ADDRDOMAIN_BITS == 12
  marshal_word(pos, addr1 | addr2 << 12); marshal_byte(pos, addr2 >> 4);
#else
  marshal_word(pos, addr1); marshal_word(pos, addr2);
#endif
}

void unmarshal_addrs(uint8_t **pos, ADDRESS *addr1, ADDRESS *addr2)
{
#if ADDRDOMAIN_BITS == 8
  *addr1 = unmarshal_byte(pos); *addr2 = unmarshal_byte(pos);
#elif ADDRDOMAIN_BITS == 12
  *addr1 = unmarshal_word(pos); *addr2 = unmarshal_byte(pos);
  *addr2 = (*addr2 << 4) | (*addr1 >> 12);
  *addr1 &= ADDR_BITMASK;
#else
  *addr1 = marshal_word(pos); *addr2 = marshal_word(pos);
#endif
}

void marshal_pathvec(uint8_t **pos, ADDRESS *path, uint8_t len)
{
  int i;

#if ADDR_BITS == 8
  for (i = 0; i < len; i++)
    marshal_byte(pos, path[i]);
#elif ADDR_BITS == 12
  for (i = 0; i < len/2; i++)
    marshal_addrs(pos, &path[2*i], &path[2*i+1]);

  if (len % 2 == 1)
    marshal_addr(pos, path[len-1]);
#else
  for (i = 0; i < len; i++)
    marshal_word(pos, path[i]);
#endif
}

void unmarshal_pathvec(uint8_t **pos, ADDRESS *path, uint8_t len)
{
  int i;

#if ADDR_BITS == 8
  for (i = 0; i < len; i++)
    path[i] = unmarshal_byte(pos);
#elif ADDR_BITS == 12
  for (i = 0; i < len/2; i++)
    unmarshal_addrs(pos, &path[2*i], &path[2*i+1]);

  if (len % 2 == 1)
    path[len-1] = unmarshal_addr(pos);
#else
  for (i = 0; i < len; i++)
    path[i] = unmarshal_word(pos);
#endif
}

void marshal_pathlevels(uint8_t **pos, uint8_t *path_levels, uint8_t path_len)
{
  int i;
  uint8_t buffer[DIV_CEIL(MAX_PATHLEN-1, 4)];

  marshal_byte(pos, path_levels[0]);
  for (i = 0; i < path_len-1; i++)
    if (path_levels[i+1] == path_levels[i]+1)
    {
      BV_SET(buffer, 2*i, 1); BV_SET(buffer, 2*i+1, 1);
    } else if (path_levels[i+1] == path_levels[i]-1)
    {
      BV_SET(buffer, 2*i, 1); BV_SET(buffer, 2*i+1, 0);
    } else if (path_levels[i+1] == path_levels[i])
    {
      BV_SET(buffer, 2*i, 0); BV_SET(buffer, 2*i+1, 0);
    } else {
      DBGERR("Cannot encode path levels! %d, %d", path_levels[i+1], path_levels[i]);
      pause_sim();
      return;
    }

  marshal_buffer(pos, buffer, DIV_CEIL(path_len-1, 4));
}

void unmarshal_pathlevels(uint8_t **pos, uint8_t *path_levels, uint8_t path_len)
{
  int i;
  uint8_t buffer[DIV_CEIL(MAX_PATHLEN-1, 4)];

  path_levels[0] = unmarshal_byte(pos);
  unmarshal_buffer(pos, buffer, DIV_CEIL(path_len-1, 4));

  for (i = 0; i < path_len-1; i++)
    switch (2*BV_GET(buffer, 2*i)+BV_GET(buffer, 2*i+1))
    {
      case 0: path_levels[i+1] = path_levels[i]; break;
      case 2: path_levels[i+1] = path_levels[i]-1; break;
      case 3: path_levels[i+1] = path_levels[i]+1; break;
      default:
        DBGERR("Invalid diff code");
        return;
    }
}

void marshal_tokenlist(uint8_t **pos, TOKEN_LIST *s)
{
  int i;
  uint8_t buffer[DIV_CEIL(MAX_QUERYTOKENS, 4)];

  for (i = 0; i < s->token_count; i++)
  {
    BV_SET(buffer, 2*i, s->token[i].type & 0x01);
    BV_SET(buffer, 2*i+1, (s->token[i].type >> 1) & 0x01);
  }
  marshal_buffer(pos, buffer, DIV_CEIL(s->token_count, 4));

  for (i = 0; i < s->token_count; i++)
    marshal_wordvb(pos, s->token[i].value);
}

void unmarshal_tokenlist(uint8_t **pos, TOKEN_LIST *s)   // NOTE: s->token_count is already set
{
  int i;
  uint8_t buffer[DIV_CEIL(MAX_QUERYTOKENS, 4)];
  ATTRIBUTE v;

  unmarshal_buffer(pos, buffer, DIV_CEIL(s->token_count, 4));
  for (i = 0; i < s->token_count; i++)
  {
    s->token[i].type = (BV_GET(buffer, 2*i) | (BV_GET(buffer, 2*i+1) << 1));
    v = unmarshal_wordvb(pos);    
    s->token[i].value = v;
    if (s->token[i].type == TOKEN_ATTR && !ATTRGET_SEQ(v) && !ATTRGET_TEMP(v))
      BV_SET(s->attr_used, ATTRGET_TYPE(v), 1);
  }
}

uint8_t marshal_discoverymsg(DISCOVERY_MSG *msg, uint8_t *buffer)
{
  uint8_t *pos = buffer;

  marshal_byte(&pos, msg->cmd | (msg->has_time << 4) | (msg->is_timesync << 5));
  
  if (msg->has_time)
  {
    marshal_long(&pos, msg->time);    // do not marshal as varbyte to allow value to be updated right before send in "update_buffer"
    marshal_wordvb(&pos, msg->main_timer);
    marshal_byte(&pos, msg->time_hops);
  }
 
  return pos-buffer;
}

uint8_t unmarshal_discoverymsg(DISCOVERY_MSG *msg, uint8_t *buffer)
{
  uint8_t *pos = buffer;

  msg->cmd = unmarshal_byte(&pos);
  msg->has_time = (msg->cmd >> 4) & 0x01;
  msg->is_timesync = (msg->cmd >> 5) & 0x01;
  
  if (msg->has_time)
  {
    msg->time = unmarshal_long(&pos);
    msg->main_timer = unmarshal_wordvb(&pos);
    msg->time_hops = unmarshal_byte(&pos);
  } else {
    msg->time = 0;
    msg->main_timer = 0;
    msg->time_hops = 0xff;
  }
  
  msg->cmd &= CMD_MASK;
  
  return pos-buffer;
}

uint8_t marshal_treeconstructmsg(TREECONSTRUCT_MSG *msg, uint8_t *buffer)
{
  uint8_t *pos = buffer;

  marshal_byte(&pos, msg->cmd | (msg->tree << 4));
  marshal_byte(&pos, msg->hops);
  marshal_addr(&pos, msg->parent);
  marshal_wordvb(&pos, msg->phase_start);

  return pos-buffer;
}

uint8_t unmarshal_treeconstructmsg(TREECONSTRUCT_MSG *msg, uint8_t *buffer)
{
  uint8_t *pos = buffer;

  msg->cmd = unmarshal_byte(&pos);
  msg->tree = (msg->cmd >> 4);
  msg->hops = unmarshal_byte(&pos);
  msg->parent = unmarshal_addr(&pos);
  msg->phase_start = unmarshal_wordvb(&pos);

  msg->cmd &= CMD_MASK;

  return pos-buffer;
}

uint8_t marshal_treecommitmsg(TREECOMMIT_MSG *msg, uint8_t *buffer)
{
  uint8_t *pos = buffer;

  marshal_byte(&pos, msg->cmd | (msg->tree << 4));

  return pos-buffer;
}

uint8_t unmarshal_treecommitmsg(TREECOMMIT_MSG *msg, uint8_t *buffer)
{
  uint8_t *pos = buffer;

  msg->cmd = unmarshal_byte(&pos);
  msg->tree = (msg->cmd >> 4);
  msg->cmd &= CMD_MASK;
  return pos-buffer;
}

uint8_t marshal_treesummarymsg(TREESUMMARY_MSG *msg, uint8_t *buffer)
{
  int i;
  uint8_t *pos = buffer;

  marshal_byte(&pos, msg->cmd | (msg->tree << 4));

  marshal_addrs(&pos, msg->deepest_id, msg->deepestsum_id);
  marshal_byte(&pos, msg->deepest_hops);
  marshal_byte(&pos, msg->deepesthops_sum);

  for (i = 0; i < summarydesc_count; i++)
    marshal_summary(&pos, &msg->summary[i]);

  return pos-buffer;
}

uint8_t unmarshal_treesummarymsg(TREESUMMARY_MSG *msg, uint8_t *buffer)
{
  int i;
  uint8_t *pos = buffer;

  msg->cmd = unmarshal_byte(&pos);
  msg->tree = (msg->cmd >> 4);

  unmarshal_addrs(&pos, &msg->deepest_id, &msg->deepestsum_id);
  msg->deepest_hops = unmarshal_byte(&pos);
  msg->deepesthops_sum = unmarshal_byte(&pos);

  for (i = 0; i < summarydesc_count; i++)
    unmarshal_summary(&pos, &msg->summary[i]);

  msg->cmd &= CMD_MASK;
  
  return pos-buffer;  
}

uint8_t marshal_treesummaryupdatemsg(TREESUMMARYUPDATE_MSG *msg, uint8_t *buffer)
{
  uint8_t *pos = buffer;

  marshal_byte(&pos, msg->cmd | (msg->is_insert << 7) | (msg->is_diff << 6));

  marshal_addrs(&pos, msg->source_node, msg->new_parentnode);
  marshal_byte(&pos, msg->summary_index);
  marshal_byte(&pos, msg->tree_index);
  marshal_byte(&pos, msg->seq_number);

  if (msg->is_insert)
    marshal_word(&pos, msg->value);
  else if (msg->is_diff)
  {
    marshal_byte(&pos, msg->diff_len);
    marshal_buffer(&pos, msg->diff, msg->diff_len);
  } else marshal_summary(&pos, &msg->summary);

  return pos-buffer;
}

uint8_t unmarshal_treesummaryupdatemsg(TREESUMMARYUPDATE_MSG *msg, uint8_t *buffer)
{
  uint8_t *pos = buffer;

  msg->cmd = unmarshal_byte(&pos);
  msg->is_insert = (msg->cmd >> 7) & 0x01;
  msg->is_diff = (msg->cmd >> 6) & 0x01;
  msg->cmd &= CMD_MASK;

  unmarshal_addrs(&pos, &msg->source_node, &msg->new_parentnode);
  msg->summary_index = unmarshal_byte(&pos);
  msg->tree_index = unmarshal_byte(&pos);
  msg->seq_number = unmarshal_byte(&pos);

  if (msg->is_insert)
    msg->value = unmarshal_word(&pos);
  else if (msg->is_diff)
  {
    msg->diff_len = unmarshal_byte(&pos);
    unmarshal_buffer(&pos, msg->diff, msg->diff_len);
  } else unmarshal_summary(&pos, &msg->summary);

  return pos-buffer;
}

uint8_t marshal_treerootnotifymsg(TREEROOTNOTIFY_MSG *msg, uint8_t *buffer)
{
  uint8_t *pos = buffer;

  marshal_byte(&pos, msg->cmd | (msg->tree << 4));
  marshal_addr(&pos, msg->notify_id);

  return pos-buffer;
}

uint8_t unmarshal_treerootnotifymsg(TREEROOTNOTIFY_MSG *msg, uint8_t *buffer)
{
  uint8_t *pos = buffer;

  msg->cmd = unmarshal_byte(&pos);
  msg->tree = (msg->cmd >> 4);
  msg->notify_id = unmarshal_addr(&pos);
  
  msg->cmd &= CMD_MASK;

  return pos-buffer;
}

uint8_t marshal_routemsg(ROUTE_MSG *msg, uint8_t *buffer)
{
  int i;
  uint8_t *pos = buffer;

  marshal_byte(&pos, msg->cmd | ((msg->cmd != CMD_FLOODROUTE || msg->top_index == msg->path_len-1) << 7) |
    ((msg->path_len < 16 && msg->data_len < 16) << 6) | (msg->is_repair << 5) | ((msg->route_id != 0) << 4));

  marshal_addr(&pos, msg->source_node);
  if (msg->route_id != 0)
    marshal_byte(&pos, msg->route_id);
  if (msg->cmd != CMD_FLOODROUTE || msg->top_index == msg->path_len-1)
    marshal_byte(&pos, msg->top_index | (msg->tree_index << 6));

  if (msg->predicate_count == 1)
  {
    marshal_byte(&pos, 0x80 | msg->predicates[0].summary_index);
    marshal_word(&pos, msg->predicates[0].value);
    if (summary_desc[msg->predicates[0].summary_index].type == SUMMARY_RTREE)
      marshal_byte(&pos, msg->predicates[0].value_ext);
  } else {
    marshal_byte(&pos, msg->predicate_count);
    for (i = 0; i < msg->predicate_count; i++)
    {
      marshal_byte(&pos, msg->predicates[i].summary_index);
      marshal_word(&pos, msg->predicates[i].value);
      if (summary_desc[msg->predicates[i].summary_index].type == SUMMARY_RTREE)
        marshal_byte(&pos, msg->predicates[i].value_ext);
    }
  }

  if (msg->path_len < 16 && msg->data_len < 16)
    marshal_byte(&pos, (msg->path_len << 4) | msg->data_len);
  else {
    marshal_byte(&pos, msg->path_len);
    marshal_byte(&pos, msg->data_len);
  }

  if (msg->path_len > 0)
    marshal_pathvec(&pos, &msg->path[1], msg->path_len-1);
  marshal_buffer(&pos, msg->data, msg->data_len);

  if (msg->is_repair)
  {
    marshal_byte(&pos, msg->repair_hoplimit);
    marshal_addr(&pos, msg->failed_node);
    marshal_byte(&pos, msg->fail_count);
    marshal_byte(&pos, msg->repair_pathlen);
    marshal_pathvec(&pos, msg->repair_path, msg->repair_pathlen);
  }

  return pos-buffer;
}

uint8_t marshalest_routemsg(ROUTE_MSG *msg)
{
  uint8_t size, *p = NULL;

  marshal_checksize = 1;
  size = marshal_routemsg(msg, p);
  marshal_checksize = 0;

  return size;
}

void dounmarshal_routemsg_head(uint8_t **pos, ROUTE_MSG *msg)
{
  int i;
  uint8_t s;

  memset(msg, 0, sizeof(*msg));
  msg->cmd = unmarshal_byte(pos);
  msg->is_repair = (msg->cmd >> 5) & 0x01;

  msg->source_node = unmarshal_addr(pos);
  msg->route_id = (msg->cmd & 0x10) ? unmarshal_byte(pos) : 0;
  if (msg->cmd & 0x80)
  {
    msg->top_index = unmarshal_byte(pos);
    msg->tree_index = (msg->top_index >> 6) & 0x03;
    msg->top_index &= 0x3f;
  } else {
    msg->top_index = 0; msg->tree_index = 0;
  }

  msg->predicate_count = unmarshal_byte(pos);
  if (msg->predicate_count & 0x80)
  {
    msg->predicates[0].summary_index = msg->predicate_count & 0x7f;
    msg->predicate_count = 1;
    msg->predicates[0].value = unmarshal_word(pos);
    if (summary_desc[msg->predicates[0].summary_index].type == SUMMARY_RTREE)
      msg->predicates[0].value_ext = unmarshal_byte(pos);
  } else for (i = 0; i < msg->predicate_count; i++)
  {
    msg->predicates[i].summary_index = unmarshal_byte(pos);
    msg->predicates[i].value = unmarshal_word(pos);
    if (summary_desc[msg->predicates[i].summary_index].type == SUMMARY_RTREE)
      msg->predicates[i].value_ext = unmarshal_byte(pos);
  }

  if (msg->cmd & 0x40)
  {
    s = unmarshal_byte(pos);
    msg->path_len = s >> 4;
    msg->data_len = s & 0x0f;
  } else {
    msg->path_len = unmarshal_byte(pos);
    msg->data_len = unmarshal_byte(pos);
  }

  msg->shortcut_index = -1;  // no shortcuts performed on this message's path yet
}

uint8_t unmarshal_routemsg_head(ROUTE_MSG *msg, uint8_t *buffer)
{                    // dont unmarshall payload and source route
  uint8_t *pos = buffer;

  dounmarshal_routemsg_head(&pos, msg);
  msg->cmd &= CMD_MASK;

  return pos-buffer;
}

uint8_t unmarshal_routemsg(ROUTE_MSG *msg, uint8_t *buffer)
{
  uint8_t *pos = buffer;

  dounmarshal_routemsg_head(&pos, msg);
  
  if (msg->path_len > 0)
  {
    unmarshal_pathvec(&pos, &msg->path[1], msg->path_len-1);
    msg->path[0] = msg->source_node;
  }
  unmarshal_buffer(&pos, msg->data, msg->data_len);

  if (msg->is_repair)
  {
    msg->repair_hoplimit = unmarshal_byte(&pos);
    msg->failed_node = unmarshal_addr(&pos);
    msg->fail_count = unmarshal_byte(&pos);
    msg->repair_pathlen = unmarshal_byte(&pos);
    unmarshal_pathvec(&pos, msg->repair_path, msg->repair_pathlen);
  }

  msg->cmd &= CMD_MASK;

  return pos-buffer;
}

uint8_t marshal_basefloodmsg(BASEFLOOD_MSG *msg, uint8_t *buffer)
{
  int i;
  uint8_t *pos = buffer;

  marshal_byte(&pos, msg->cmd | (msg->predicate_count << 4));
  for (i = 0; i < msg->predicate_count; i++)
  {
    marshal_byte(&pos, msg->predicates[i].summary_index);
    marshal_word(&pos, msg->predicates[i].value);
    if (summary_desc[msg->predicates[i].summary_index].type == SUMMARY_RTREE)
      marshal_byte(&pos, msg->predicates[i].value_ext);
  }

  marshal_addr(&pos, msg->source_node);
  marshal_byte(&pos, msg->data_len);
  marshal_buffer(&pos, msg->data, msg->data_len);

  return pos-buffer;
}

uint8_t unmarshal_basefloodmsg(BASEFLOOD_MSG *msg, uint8_t *buffer)
{
  int i;
  uint8_t *pos = buffer;

  memset(msg, 0, sizeof(*msg));
  msg->cmd = unmarshal_byte(&pos);
  msg->predicate_count = (msg->cmd >> 4) & 0x0f;
  for (i = 0; i < msg->predicate_count; i++)
  {
    msg->predicates[i].summary_index = unmarshal_byte(&pos);
    msg->predicates[i].value = unmarshal_word(&pos);
    if (summary_desc[msg->predicates[i].summary_index].type == SUMMARY_RTREE)
      msg->predicates[i].value_ext = unmarshal_byte(&pos);
  }
  
  msg->source_node = unmarshal_addr(&pos);
  msg->data_len = unmarshal_byte(&pos);
  unmarshal_buffer(&pos, msg->data, msg->data_len);

  msg->cmd &= CMDHI_MASK;

  return pos-buffer;
}

uint8_t marshal_forwardmsg(FORWARD_MSG *msg, uint8_t *buffer)
{
  uint8_t *pos = buffer;

  marshal_byte(&pos, msg->cmd | ((msg->path_len < 16 && msg->data_len < 16) << 7) |
    ((msg->multicastbuffer_len > 0) << 6) | (msg->destructive << 5) | (msg->failed << 4));

  marshal_addrs(&pos, msg->source_node, msg->dest_node);

  if (msg->path_len < 16 && msg->data_len < 16)
    marshal_byte(&pos, (msg->path_len << 4) | msg->data_len);
  else {
    marshal_byte(&pos, msg->path_len);
    marshal_byte(&pos, msg->data_len);
  }

  marshal_buffer(&pos, msg->data, msg->data_len);  // make sure command byte is in first fragment, need for snooping
  if (msg->destructive == 0)
    marshal_pathvec(&pos, msg->path, msg->path_len);
  else if (msg->path_len > 0)
    marshal_pathvec(&pos, msg->path, msg->path_len-1);

  if (msg->multicastbuffer_len > 0)
  {
    marshal_byte(&pos, msg->multicastbuffer_len);
    marshal_buffer(&pos, msg->multicast_buffer, msg->multicastbuffer_len);
  }

  return pos-buffer;
}

uint8_t marshalest_forwardmsg(FORWARD_MSG *msg)
{
  uint8_t size, *p = NULL;

  marshal_checksize = 1;
  size = marshal_forwardmsg(msg, p);
  marshal_checksize = 0;

  return size;
}

void dounmarshal_forwardmsg_head(uint8_t **pos, FORWARD_MSG *msg)
{
  uint8_t s;
  
  memset(msg, 0, sizeof(*msg));
  msg->cmd = unmarshal_byte(pos);

  unmarshal_addrs(pos, &msg->source_node, &msg->dest_node);
  msg->destructive = (msg->cmd & 0x20) != 0;
  msg->failed = (msg->cmd & 0x10) != 0;

  if (msg->cmd & 0x80)
  {
    s = unmarshal_byte(pos);
    msg->path_len = s >> 4;
    msg->data_len = s & 0x0f;
  } else {
    msg->path_len = unmarshal_byte(pos);
    msg->data_len = unmarshal_byte(pos);
  }

  if (msg->data_len > 0)
    msg->data[0] = unmarshal_byte(pos);
}

uint8_t unmarshal_forwardmsg(FORWARD_MSG *msg, uint8_t *buffer)
{
  uint8_t *pos = buffer;

  dounmarshal_forwardmsg_head(&pos, msg);

  if (msg->data_len > 0)
    unmarshal_buffer(&pos, &msg->data[1], msg->data_len-1);

  if (msg->destructive == 0)
    unmarshal_pathvec(&pos, msg->path, msg->path_len);
  else if (msg->path_len > 0)
  {
    unmarshal_pathvec(&pos, msg->path, msg->path_len-1);
    msg->path[msg->path_len-1] = msg->dest_node;
  }

  if (msg->cmd & 0x40)
  {
    msg->multicastbuffer_len = unmarshal_byte(&pos);
    unmarshal_buffer(&pos, msg->multicast_buffer, msg->multicastbuffer_len);
  } else
    msg->multicastbuffer_len = 0;

  msg->cmd &= CMD_MASK;

  return pos-buffer;
}

uint8_t unmarshal_forwardmsg_head(FORWARD_MSG *msg, uint8_t *buffer)
{
  uint8_t *pos = buffer;

  dounmarshal_forwardmsg_head(&pos, msg);
  msg->cmd &= CMD_MASK;

  return pos-buffer;
}

uint8_t marshal_forwardbufferexchangemsg(FORWARDBUFFEREXCHANGE_MSG *msg, uint8_t *buffer)
{
  int i;
  uint8_t *pos = buffer;

  marshal_byte(&pos, msg->cmd | (msg->entry_count << 5));

  for (i = 0; i < msg->entry_count; i++)
  {
    marshal_addrs(&pos, msg->entries[i].source_node, msg->entries[i].dest_node);
    marshal_addr(&pos, msg->entries[i].next_node);
    marshal_byte(&pos, msg->entries[i].hops);
  }

  return pos-buffer;
}

uint8_t unmarshal_forwardbufferexchangemsg(FORWARDBUFFEREXCHANGE_MSG *msg, uint8_t *buffer)
{
  int i;
  uint8_t *pos = buffer;

  memset(msg, 0, sizeof(*msg));

  msg->cmd = unmarshal_byte(&pos);
  msg->entry_count = (msg->cmd >> 5) & 0x07;

  for (i = 0; i < msg->entry_count; i++)
  {
    unmarshal_addrs(&pos, &msg->entries[i].source_node, &msg->entries[i].dest_node);
    msg->entries[i].next_node = unmarshal_addr(&pos);
    msg->entries[i].hops = unmarshal_byte(&pos);
  }
  
  msg->cmd &= CMD_MASK;

  return pos-buffer;
}

uint8_t marshal_pathsubscribemsg(PATHSUBSCRIBE_MSG *msg, uint8_t *buffer)
{
  uint8_t *pos = buffer;

  marshal_byte(&pos, msg->cmd);
  marshal_addrs(&pos, msg->source_node1, msg->source_node2);
  marshal_addr(&pos, msg->dest_node);

  return pos-buffer;
}

uint8_t marshalest_pathsubscribemsg(PATHSUBSCRIBE_MSG *msg)
{
  uint8_t size, *p = NULL;

  marshal_checksize = 1;
  size = marshal_pathsubscribemsg(msg, p);
  marshal_checksize = 0;

  return size;
}

uint8_t unmarshal_pathsubscribemsg(PATHSUBSCRIBE_MSG *msg, uint8_t *buffer)
{
  uint8_t *pos = buffer;

  memset(msg, 0, sizeof(*msg));

  msg->cmd = unmarshal_byte(&pos);
  unmarshal_addrs(&pos, &msg->source_node1, &msg->source_node2);
  msg->dest_node = unmarshal_addr(&pos);

  return pos-buffer;
}

uint8_t marshal_pathrespondmsg(PATHRESPOND_MSG *msg, uint8_t *buffer)
{
  uint8_t *pos = buffer;

  marshal_byte(&pos, msg->cmd);

  marshal_addrs(&pos, msg->source_node1, msg->source_node2);
  marshal_addr(&pos, msg->dest_node);
  marshal_byte(&pos, msg->path_len);
  marshal_pathvec(&pos, msg->path, msg->path_len);

  return pos-buffer;
}

uint8_t unmarshal_pathrespondmsg(PATHRESPOND_MSG *msg, uint8_t *buffer)
{
  uint8_t *pos = buffer;

  memset(msg, 0, sizeof(*msg));

  msg->cmd = unmarshal_byte(&pos);

  unmarshal_addrs(&pos, &msg->source_node1, &msg->source_node2);
  msg->dest_node = unmarshal_addr(&pos);
  msg->path_len = unmarshal_byte(&pos);
  unmarshal_pathvec(&pos, msg->path, msg->path_len);

  return pos-buffer;
}

uint8_t marshal_initmsghi(INIT_MSGHI *msg, uint8_t *buffer)
{
  int i;
  uint8_t *pos = buffer;

  marshal_byte(&pos, msg->cmd | ((msg->path_len < 8) << 7) | ((msg->path_len & 0x07) << 4));

  marshal_byte(&pos, msg->query_id);
  if (msg->path_len >= 8)
    marshal_byte(&pos, msg->path_len);
  marshal_pathlevels(&pos, msg->path_levels, msg->path_len);

  marshal_byte(&pos, msg->eval_count);
  for (i = 0; i < msg->eval_count; i++)
    marshal_wordvb(&pos, msg->eval[i]);

  return pos-buffer;
}

uint8_t unmarshal_initmsghi(INIT_MSGHI *msg, uint8_t *buffer)
{
  int i;
  uint8_t *pos = buffer;

  memset(msg, 0, sizeof(*msg));
  msg->cmd = unmarshal_byte(&pos);

  msg->query_id = unmarshal_byte(&pos);
  msg->path_len = (msg->cmd & 0x80) ? (msg->cmd >> 4) & 0x07 : unmarshal_byte(&pos);
  unmarshal_pathlevels(&pos, msg->path_levels, msg->path_len);

  msg->eval_count = unmarshal_byte(&pos);
  for (i = 0; i < msg->eval_count; i++)
    msg->eval[i] = unmarshal_wordvb(&pos);

  msg->cmd &= CMDHI_MASK;
  return pos-buffer;
}

uint8_t marshal_confirmjoinmsghi(CONFIRMJOIN_MSGHI *msg, uint8_t *buffer)
{
  int i;
  uint8_t *pos = buffer;

  marshal_byte(&pos, msg->cmd | (msg->sel_update << 7) | (msg->remove_pair << 6));

  marshal_byte(&pos, msg->query_id);
  marshal_addr(&pos, msg->source_node);

  if (msg->remove_pair == 0)
  {
    marshal_byte(&pos, msg->seq_number);
    marshal_byte(&pos, msg->path_nodecount);
    marshal_pathvec(&pos, msg->path_nodes, msg->path_nodecount);
    if (msg->sel_update)
      for (i = 0; i < REL_COUNT; i++)
        marshal_byte(&pos, msg->sel_est[i]);
  }
  
  return pos-buffer;
}

uint8_t unmarshal_confirmjoinmsghi(CONFIRMJOIN_MSGHI *msg, uint8_t *buffer)
{
  int i;
  uint8_t *pos = buffer;

  memset(msg, 0, sizeof(*msg));
  msg->cmd = unmarshal_byte(&pos);
  msg->sel_update = (msg->cmd >> 7) & 0x01;
  msg->remove_pair = (msg->cmd >> 6) & 0x01;

  msg->query_id = unmarshal_byte(&pos);
  msg->source_node = unmarshal_addr(&pos);

  if (msg->remove_pair == 0)
  {
    msg->seq_number = unmarshal_byte(&pos);
    msg->path_nodecount = unmarshal_byte(&pos);
    unmarshal_pathvec(&pos, msg->path_nodes, msg->path_nodecount);
    if (msg->sel_update)
      for (i = 0; i < REL_COUNT; i++)
        msg->sel_est[i] = unmarshal_byte(&pos);
  }

  msg->cmd &= CMDHI_MASK;
  
  return pos-buffer;
}

uint8_t marshal_confirmsourcemsghi(CONFIRMSOURCE_MSGHI *msg, uint8_t *buffer)
{
  int i;
  uint8_t *pos = buffer;

  marshal_byte(&pos, msg->cmd | (msg->sel_update << 7));

  marshal_byte(&pos, msg->query_id);
  marshal_addr(&pos, msg->target_node);
  marshal_byte(&pos, msg->seq_number);
  marshal_byte(&pos, msg->level);
  if (msg->sel_update)
    for (i = 0; i < REL_COUNT; i++)
      marshal_byte(&pos, msg->sel_est[i]);
    
  return pos-buffer;
}

uint8_t unmarshal_confirmsourcemsghi(CONFIRMSOURCE_MSGHI *msg, uint8_t *buffer)
{
  int i;
  uint8_t *pos = buffer;

  memset(msg, 0, sizeof(*msg));
  msg->cmd = unmarshal_byte(&pos);
  msg->sel_update = (msg->cmd >> 7) & 0x01;

  msg->query_id = unmarshal_byte(&pos);
  msg->target_node = unmarshal_addr(&pos);
  msg->seq_number = unmarshal_byte(&pos);
  msg->level = unmarshal_byte(&pos);
  if (msg->sel_update)
    for (i = 0; i < REL_COUNT; i++)
      msg->sel_est[i] = unmarshal_byte(&pos);

  msg->cmd &= CMDHI_MASK;

  return pos-buffer;
}

uint8_t marshal_confirmtargetsourcemsghi(CONFIRMTARGETSOURCE_MSGHI *msg, uint8_t *buffer)
{
  int i;
  uint8_t *pos = buffer;

  marshal_byte(&pos, msg->cmd | (msg->sel_update << 7));
  marshal_byte(&pos, msg->query_id);
  marshal_byte(&pos, msg->seq_number);
  if (msg->sel_update)
    for (i = 0; i < REL_COUNT; i++)
      marshal_byte(&pos, msg->sel_est[i]);

  return pos-buffer;
}

uint8_t unmarshal_confirmtargetsourcemsghi(CONFIRMTARGETSOURCE_MSGHI *msg, uint8_t *buffer)
{
  int i;
  uint8_t *pos = buffer;

  memset(msg, 0, sizeof(*msg));
  msg->cmd = unmarshal_byte(&pos);
  msg->sel_update = (msg->cmd >> 7) & 0x01;
  msg->query_id = unmarshal_byte(&pos);
  msg->seq_number = unmarshal_byte(&pos);
  if (msg->sel_update)
    for (i = 0; i < REL_COUNT; i++)
      msg->sel_est[i] = unmarshal_byte(&pos);

  msg->cmd &= CMDHI_MASK;

  return pos-buffer;
}

uint8_t marshal_tuplemsghi(TUPLE_MSGHI *msg, uint8_t *buffer)
{
  uint8_t *pos = buffer;

  marshal_byte(&pos, msg->cmd | (msg->cache_multicast << 6));
  marshal_page(&pos, &msg->page);

  return pos-buffer;
}

uint8_t marshalest_tuplemsghi(TUPLE_MSGHI *msg)
{
  uint8_t size, *p = NULL;

  marshal_checksize = 1;
  size = marshal_tuplemsghi(msg, p);
  marshal_checksize = 0;

  return size;
}

uint8_t unmarshal_tuplemsghi(TUPLE_MSGHI *msg, uint8_t *buffer, ADDRESS source_node)
{
  uint8_t *pos = buffer;

  memset(msg, 0, sizeof(*msg));
  msg->cmd = unmarshal_byte(&pos);
  msg->cache_multicast = ((msg->cmd >> 6) & 0x03);
  msg->cmd &= CMDHI_MASK;
  unmarshal_page(&pos, &msg->page, source_node);

  return pos-buffer;
}

uint8_t marshal_repairmsghi(REPAIR_MSGHI *msg, uint8_t *buffer)
{
  uint8_t *pos = buffer;

  marshal_byte(&pos, msg->cmd | (msg->path_found << 7));
  marshal_byte(&pos, msg->path_index);
  marshal_addrs(&pos, msg->neighbor_node, msg->dest_node);

  return pos-buffer;
}

uint8_t unmarshal_repairmsghi(REPAIR_MSGHI *msg, uint8_t *buffer)
{
  uint8_t *pos = buffer;

  memset(msg, 0, sizeof(*msg));

  msg->cmd = unmarshal_byte(&pos);
  msg->path_found = (msg->cmd & 0x80) != 0;
  msg->cmd &= CMDHI_MASK;

  msg->path_index = unmarshal_byte(&pos);
  unmarshal_addrs(&pos, &msg->neighbor_node, &msg->dest_node);

  return pos-buffer;
}

uint8_t marshal_estupdatemsghi(ESTUPDATE_MSGHI *msg, uint8_t *buffer)
{
  int i;
  uint8_t *pos = buffer;

  marshal_byte(&pos, msg->cmd);
  marshal_byte(&pos, msg->query_id);
  marshal_addr(&pos, msg->source_node);
  for (i = 0; i < REL_COUNT; i++)
    marshal_byte(&pos, msg->sel_est[i]);

  return pos-buffer;
}

uint8_t unmarshal_estupdatemsghi(ESTUPDATE_MSGHI *msg, uint8_t *buffer)
{
  int i;
  uint8_t *pos = buffer;

  memset(msg, 0, sizeof(*msg));
  msg->cmd = unmarshal_byte(&pos);
  msg->query_id = unmarshal_byte(&pos);
  msg->source_node = unmarshal_addr(&pos);
  for (i = 0; i < REL_COUNT; i++)
    msg->sel_est[i] = unmarshal_byte(&pos);

  return pos-buffer;
}

uint8_t marshal_attrupdatemsghi(ATTRUPDATE_MSGHI *msg, uint8_t *buffer)
{
  int i;
  uint8_t *pos = buffer;

  marshal_byte(&pos, msg->cmd);
  marshal_byte(&pos, msg->node_count | (msg->attr_index << 4));

  for (i = 0; i < msg->node_count/2; i++)
    marshal_addrs(&pos, msg->nodes[2*i].id, msg->nodes[2*i+1].id);
  if (msg->node_count % 2)
    marshal_addr(&pos, msg->nodes[msg->node_count-1].id);

  for (i = 0; i < msg->node_count; i++)
    marshal_wordvb(&pos, msg->nodes[i].value);

  return pos-buffer;
}

uint8_t unmarshal_attrupdatemsghi(ATTRUPDATE_MSGHI *msg, uint8_t *buffer)
{
  int i;
  uint8_t *pos = buffer;

  memset(msg, 0, sizeof(*msg));
  msg->cmd = unmarshal_byte(&pos);
  msg->node_count = unmarshal_byte(&pos);
  msg->attr_index = (msg->node_count >> 4);
  msg->node_count &= 0x0f;

  for (i = 0; i < msg->node_count/2; i++)
    unmarshal_addrs(&pos, &msg->nodes[2*i].id, &msg->nodes[2*i+1].id);
  if (msg->node_count % 2)
    msg->nodes[msg->node_count-1].id = unmarshal_addr(&pos);

  for (i = 0; i < msg->node_count; i++)
    msg->nodes[i].value = unmarshal_wordvb(&pos);

  return pos-buffer;
}

void marshal_stack(uint8_t **pos, STACK *s)
{
  marshal_byte(pos, s->stack_len);
}

void unmarshal_stack(uint8_t **pos, STACK *s, TOKEN **l)
{
  s->stack_len = unmarshal_byte(pos);
  s->stack = *l;
  (*l) += s->stack_len;
}

void marshal_queryspec(uint8_t **pos, QUERYSPEC_ENTRY *spec)
{
  int i, j, k;
  uint8_t buffer[DIV_CEIL(MAX_EVALS, 8)];
  
  marshal_byte(pos, spec->srcrel_count);
  for (i = REL_STATIC; i <= REL_DYNAMIC; i++)
    for (j = 0; j < REL_COUNT; j++)
    {
      marshal_stack(pos, &spec->rel[i][j].pred);
 
      marshal_byte(pos, spec->rel[i][j].eval_count | ((spec->rel[i][j].id_index >= 0) << 4));
      if (spec->rel[i][j].eval_count > 0)
      {
        if (spec->rel[i][j].id_index >= 0)
          marshal_byte(pos, spec->rel[i][j].id_index);
        if (i == REL_STATIC && j == REL_J)
          marshal_byte(pos, spec->rel[i][j].eval_aggrcount);

        memset(buffer, 0, sizeof(buffer));
        for (k = 0; k < spec->rel[i][j].eval_count; k++)
        {
          marshal_stack(pos, &spec->rel[i][j].eval[k].eval);
          BV_SET(buffer, k, spec->rel[i][j].eval[k].is_aggr);
        }
        marshal_buffer(pos, buffer, DIV_CEIL(spec->rel[i][j].eval_count, 8));
      }
    }

  marshal_byte(pos, spec->routingpredicate_count);
  for (i = 0; i < spec->routingpredicate_count; i++)
  {
    marshal_stack(pos, &spec->routing_predicates[i].pred);
    marshal_byte(pos, spec->routing_predicates[i].summary_index);
    marshal_byte(pos, spec->routing_predicates[i].lastchild_ofs);
  }

  marshal_tokenlist(pos, &spec->token_list);
  
  for (i = 0; i < PARAM_COUNT; i++)
    marshal_wordvb(pos, spec->params[i]);
  for (i = 0; i < REL_COUNT; i++)
    marshal_byte(pos, spec->sel_est[i]);
}

void unmarshal_queryspec(uint8_t **pos, QUERYSPEC_ENTRY *spec)
{
  int i, j, k;
  uint8_t buffer[DIV_CEIL(MAX_EVALS, 8)];
  TOKEN *l;

  memset(spec, 0, sizeof(*spec));
  
  l = spec->token_list.token;
  
  spec->srcrel_count = unmarshal_byte(pos);
  for (i = REL_STATIC; i <= REL_DYNAMIC; i++)
    for (j = 0; j < REL_COUNT; j++)
    {
      unmarshal_stack(pos, &spec->rel[i][j].pred, &l);
      spec->rel[i][j].eval_count = unmarshal_byte(pos);
      spec->rel[i][j].id_index = ((spec->rel[i][j].eval_count >> 4) ? 0 : -1);
      spec->rel[i][j].eval_count &= 0x0f;
      if (spec->rel[i][j].eval_count > 0)
      {
        if (spec->rel[i][j].id_index == 0)
          spec->rel[i][j].id_index = unmarshal_byte(pos);
        spec->rel[i][j].eval_aggrcount = ((i == REL_STATIC && j == REL_J) ? unmarshal_byte(pos) : 0);

        for (k = 0; k < spec->rel[i][j].eval_count; k++)
          unmarshal_stack(pos, &spec->rel[i][j].eval[k].eval, &l);

        unmarshal_buffer(pos, buffer, DIV_CEIL(spec->rel[i][j].eval_count, 8));
        for (k = 0; k < spec->rel[i][j].eval_count; k++)
          spec->rel[i][j].eval[k].is_aggr = BV_GET(buffer, k);
      }
    }

  spec->routingpredicate_count = unmarshal_byte(pos);
  for (i = 0; i < spec->routingpredicate_count; i++)
  {
    unmarshal_stack(pos, &spec->routing_predicates[i].pred, &l);
    spec->routing_predicates[i].summary_index = unmarshal_byte(pos);
    spec->routing_predicates[i].lastchild_ofs = unmarshal_byte(pos);
  }

  spec->token_list.token_count = l-spec->token_list.token;
  unmarshal_tokenlist(pos, &spec->token_list);
  
  for (i = 0; i < PARAM_COUNT; i++)
    spec->params[i] = unmarshal_wordvb(pos);
  for (i = 0; i < REL_COUNT; i++)
    spec->sel_est[i] = unmarshal_byte(pos);
}

uint8_t marshal_queryspecmsghi(QUERYSPEC_MSGHI *msg, uint8_t *buffer)
{
  uint8_t *pos = buffer;

  marshal_byte(&pos, msg->cmd | (msg->running << 7));
  marshal_byte(&pos, msg->query_id);
  if (msg->running)
    marshal_queryspec(&pos, &msg->spec);

  return pos-buffer;
}

uint8_t marshal_queryspecpmsghi(QUERYSPECP_MSGHI *msg, uint8_t *buffer)
{
  uint8_t *pos = buffer;

  marshal_byte(&pos, msg->cmd | (msg->running << 7));
  marshal_byte(&pos, msg->query_id);
  if (msg->running && msg->spec != NULL)
    marshal_queryspec(&pos, msg->spec);

  return pos-buffer;
}

uint8_t marshalest_queryspecpmsghi(QUERYSPECP_MSGHI *msg)
{
  uint8_t size, *p = NULL;

  marshal_checksize = 1;
  size = marshal_queryspecpmsghi(msg, p);
  marshal_checksize = 0;

  return size;
}

uint8_t unmarshal_queryspecmsghi(QUERYSPEC_MSGHI *msg, uint8_t *buffer)
{
  uint8_t *pos = buffer;

  memset(msg, 0, sizeof(*msg));
  msg->cmd = unmarshal_byte(&pos);         
  msg->running = ((msg->cmd >> 7) & 0x01);
  msg->cmd &= CMDHI_MASK;
  msg->query_id = unmarshal_byte(&pos);
  if (msg->running)
    unmarshal_queryspec(&pos, &msg->spec);

  return pos-buffer;
}

uint8_t unmarshal_queryspecpmsghi(QUERYSPECP_MSGHI *msg, uint8_t *buffer)
{
  uint8_t *pos = buffer;

  msg->cmd = unmarshal_byte(&pos);               // do not memset to 0, will erase spec pointer         
  msg->running = ((msg->cmd >> 7) & 0x01);
  msg->cmd &= CMDHI_MASK;
  msg->query_id = unmarshal_byte(&pos);
  if (msg->running && msg->spec != NULL)
    unmarshal_queryspec(&pos, msg->spec);

  return pos-buffer;
}

uint8_t marshal_summaryspecmsghi(SUMMARYSPEC_MSGHI *msg, uint8_t *buffer)
{
  int i;
  uint8_t *pos = buffer;

  marshal_byte(&pos, msg->cmd);
  marshal_buffer(&pos, msg->attr_static, DIV_CEIL(MAX_ATTRDESC, 8));
  marshal_byte(&pos, msg->summarydesc_count);
  for (i = 0; i < msg->summarydesc_count; i++)
  {
    marshal_byte(&pos, msg->summary_desc[i].attrdesc_index);
    marshal_byte(&pos, msg->summary_desc[i].type);
    marshal_byte(&pos, msg->summary_desc[i].options);
  }

  return pos-buffer;
}

uint8_t unmarshal_summaryspecmsghi(SUMMARYSPEC_MSGHI *msg, uint8_t *buffer)
{
  int i;
  uint8_t *pos = buffer;

  memset(msg, 0, sizeof(*msg));

  msg->cmd = unmarshal_byte(&pos);
  unmarshal_buffer(&pos, msg->attr_static, DIV_CEIL(MAX_ATTRDESC, 8));
  msg->summarydesc_count = unmarshal_byte(&pos);
  for (i = 0; i < msg->summarydesc_count; i++)
  {
    msg->summary_desc[i].attrdesc_index = unmarshal_byte(&pos);
    msg->summary_desc[i].type = unmarshal_byte(&pos);
    msg->summary_desc[i].options = unmarshal_byte(&pos);
  }

  return pos-buffer;
}

uint8_t marshal_pathcollapsemsghi(PATHCOLLAPSE_MSGHI *msg, uint8_t *buffer)
{
  int i;
  uint8_t *pos = buffer;

  marshal_byte(&pos, msg->cmd | (msg->hint_count << 4));
  for (i = 0; i < msg->hint_count; i++)
  {
    if (msg->hint_count > 1)
      marshal_addr(&pos, msg->hints[i].source_node);
    marshal_addrs(&pos, msg->hints[i].dest_node1, msg->hints[i].neighbor_node);
    marshal_byte(&pos, msg->hints[i].path_len);
    marshal_pathvec(&pos, msg->hints[i].path, msg->hints[i].path_len);
    if (msg->hints[i].path_len == 0)
      marshal_addr(&pos, msg->hints[i].dest_node2);
  }

  return pos-buffer;
}

uint8_t marshalest_pathcollapsemsghi(PATHCOLLAPSE_MSGHI *msg)
{
  uint8_t size, *p = NULL;

  marshal_checksize = 1;
  size = marshal_pathcollapsemsghi(msg, p);
  marshal_checksize = 0;

  return size;
}

uint8_t unmarshal_pathcollapsemsghi(PATHCOLLAPSE_MSGHI *msg, uint8_t *buffer, ADDRESS source_node)
{
  int i;
  uint8_t *pos = buffer;

  memset(msg, 0, sizeof(*msg));

  msg->cmd = unmarshal_byte(&pos);
  msg->hint_count = (msg->cmd >> 4) & 0x07;
  msg->cmd &= CMDHI_MASK;
  for (i = 0; i < msg->hint_count; i++)
  {
    msg->hints[i].source_node = (msg->hint_count > 1) ? unmarshal_addr(&pos) : source_node;
    unmarshal_addrs(&pos, &msg->hints[i].dest_node1, &msg->hints[i].neighbor_node);
    msg->hints[i].path_len = unmarshal_byte(&pos);
    unmarshal_pathvec(&pos, msg->hints[i].path, msg->hints[i].path_len);
    if (msg->hints[i].path_len == 0)
      msg->hints[i].dest_node2 = unmarshal_addr(&pos);
  }

  return pos-buffer;
}

uint8_t marshal_coordcostmsghi(COORDCOST_MSGHI *msg, uint8_t *buffer)
{
  int i;
  uint8_t *pos = buffer;
  uint32_t diff;

  marshal_byte(&pos, msg->cmd | ((msg->node_count < 8) << 7) | ((msg->node_count & 0x07) << 4));
  marshal_byte(&pos, msg->query_id);

  if (msg->node_count >= 8)
    marshal_byte(&pos, msg->node_count);
  for (i = 0; i < msg->node_count; i++)
  {
    marshal_byte(&pos, msg->nodes[i].cost.plancost_seq);
    diff = INT_TO_UINT(msg->nodes[i].cost.plancost_diff);
    marshal_wordvb(&pos, diff);
  }
  for (i = 0; i < msg->node_count/2; i++)
    marshal_addrs(&pos, msg->nodes[2*i].id, msg->nodes[2*i+1].id);
  if (msg->node_count % 2)
    marshal_addr(&pos, msg->nodes[msg->node_count-1].id);

  return pos-buffer;
}

uint8_t marshalest_coordcostmsghi(COORDCOST_MSGHI *msg)
{
  uint8_t size, *p = NULL;

  marshal_checksize = 1;
  size = marshal_coordcostmsghi(msg, p);
  marshal_checksize = 0;

  return size;
}

uint8_t unmarshal_coordcostmsghi(COORDCOST_MSGHI *msg, uint8_t *buffer)
{
  int i;
  uint8_t *pos = buffer;
  uint32_t diff;

  memset(msg, 0, sizeof(*msg));
  msg->cmd = unmarshal_byte(&pos);
  msg->query_id = unmarshal_byte(&pos);

  msg->node_count = (msg->cmd & 0x80) ? (msg->cmd >> 4) & 0x07 : unmarshal_byte(&pos);
  for (i = 0; i < msg->node_count; i++)
  {
    msg->nodes[i].cost.plancost_seq = unmarshal_byte(&pos);
    diff = unmarshal_wordvb(&pos);
    msg->nodes[i].cost.plancost_diff = UINT_TO_INT(diff);
  }
  for (i = 0; i < msg->node_count/2; i++)
    unmarshal_addrs(&pos, &msg->nodes[2*i].id, &msg->nodes[2*i+1].id);
  if (msg->node_count % 2)
    msg->nodes[msg->node_count-1].id = unmarshal_addr(&pos);

  msg->cmd &= CMDHI_MASK;

  return pos-buffer;
}

uint8_t marshal_coorddecisionmsghi(COORDDECISION_MSGHI *msg, uint8_t *buffer)
{
  uint8_t *pos = buffer;

  marshal_byte(&pos, msg->cmd | (msg->decision.decision << 7) | (msg->forward_decision << 6));
  marshal_byte(&pos, msg->query_id);
  marshal_byte(&pos, msg->decision.decision_seq);

  return pos-buffer;
}

uint8_t unmarshal_coorddecisionmsghi(COORDDECISION_MSGHI *msg, uint8_t *buffer)
{
  uint8_t *pos = buffer;

  memset(msg, 0, sizeof(*msg));
  msg->cmd = unmarshal_byte(&pos);
  msg->query_id = unmarshal_byte(&pos);
  msg->decision.decision = (msg->cmd >> 7) & 0x01;
  msg->forward_decision = (msg->cmd >> 6) & 0x01;
  msg->decision.decision_seq = unmarshal_byte(&pos);

  msg->cmd &= CMDHI_MASK;

  return pos-buffer;
}

uint8_t marshal_coordpairsmsghi(COORDPAIRS_MSGHI *msg, uint8_t *buffer)
{
  uint8_t *pos = buffer;

  marshal_byte(&pos, msg->cmd | (msg->remove_pairs << 7));
  marshal_byte(&pos, msg->query_id);
  marshal_byte(&pos, msg->source_count);
  marshal_pathvec(&pos, msg->source_nodes, msg->source_count);
  marshal_byte(&pos, msg->target_count);
  marshal_pathvec(&pos, msg->target_nodes, msg->target_count);

  return pos-buffer;
}

uint8_t unmarshal_coordpairsmsghi(COORDPAIRS_MSGHI *msg, uint8_t *buffer)
{
  uint8_t *pos = buffer;

  memset(msg, 0, sizeof(*msg));
  msg->cmd = unmarshal_byte(&pos);
  msg->remove_pairs = (msg->cmd >> 7) & 0x01;

  msg->query_id = unmarshal_byte(&pos);
  msg->source_count = unmarshal_byte(&pos);
  unmarshal_pathvec(&pos, msg->source_nodes, msg->source_count);
  msg->target_count = unmarshal_byte(&pos);
  unmarshal_pathvec(&pos, msg->target_nodes, msg->target_count);

  msg->cmd &= CMDHI_MASK;
  
  return pos-buffer;
}

uint8_t marshal_statsmsghi(STATS_MSGHI *msg, uint8_t *buffer)
{
  uint8_t *pos = buffer;

  marshal_byte(&pos, msg->cmd | (msg->type << 4));
  if (msg->type == STATTYPE_RESPONSE)
  {
    marshal_byte(&pos, msg->fragment_count);
    marshal_byte(&pos, msg->fragment_index);
    marshal_byte(&pos, msg->stat_size);
    marshal_buffer(&pos, msg->stats, msg->stat_size);
  } else marshal_word(&pos, msg->dest_node);   // not marshal_addr because dest_node can be AM_BROADCAST_ADDR
  
  return pos-buffer;
}

uint8_t unmarshal_statsmsghi(STATS_MSGHI *msg, uint8_t *buffer)
{
  uint8_t *pos = buffer;

  memset(msg, 0, sizeof(*msg));
  msg->cmd = unmarshal_byte(&pos);
  msg->type = (msg->cmd >> 4);
  if (msg->type == STATTYPE_RESPONSE)
  {
    msg->fragment_count = unmarshal_byte(&pos);
    msg->fragment_index = unmarshal_byte(&pos);
    msg->stat_size = unmarshal_byte(&pos);
    unmarshal_buffer(&pos, msg->stats, msg->stat_size);
  } else msg->dest_node = unmarshal_word(&pos);  // not marshal_addr because dest_node can be AM_BROADCAST_ADDR
  
  msg->cmd &= CMDHI_MASK;

  return pos-buffer;
}


